﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.IO;
using NDtw;

namespace Microsoft.Samples.Kinect.SkeletonBasics
{

    
    class Gesture
    {

        private String gestureName;
        private List<List<float>> pcs;
        private List<float> columnMeans;
        private GestureRegressionModel gestureModel;
        private int modelLength;
        private int endThresholdTime;
        private int startThresholdTime;
        private String dimensionsUsed;
        private float aveError;
        private float GMRthreshold;
        private float DTWThreshold;
        private double DTWValue;

        public Gesture(String name, String PCfilename, string COLfilename, string model, string covs, int startTime, int endTime, String dimUsed, float t, float dtw) {

            gestureName = name;
            pcs = new List<List<float>>();
            columnMeans = new List<float>();
            endThresholdTime = endTime;
            startThresholdTime = startTime;
            dimensionsUsed = dimUsed;
            GMRthreshold = t;
            DTWThreshold = dtw;

            Console.Out.WriteLine("reading PC file");
            StreamReader reader = new StreamReader(PCfilename);
            this.readPCInfo(reader);

            Console.Out.WriteLine("reading column means file");
            reader = new StreamReader(COLfilename);
            this.readColMeansInfo(reader);

            //Console.Out.WriteLine("reading covariances");
            //reader = new StreamReader(covs);
            //this.readCovariances(reader);

            this.createModel(model, covs);


            //Console.Out.WriteLine("PCs");
            //foreach (List<float> pc in pcs)
            //{
            //    foreach (float d in pc)
            //    {
            //        Console.Out.Write(d + " ");
            //    }
            //    Console.Out.WriteLine();
            //    Console.Out.WriteLine("Count " + pc.Count());
            //}

            //Console.Out.WriteLine("Column means");
            //foreach (double d in columnMeans)
            //{

            //    Console.Out.Write(d);
            //}


            //Console.Out.WriteLine("Covariances");
            //foreach (List<float> cov in covariances)
            //{
            //    foreach (float f in cov)
            //    {
            //        Console.Out.Write(f + " ");
            //    }
            //    Console.Out.WriteLine();
            //    Console.Out.WriteLine("Count " + cov.Count());
            //}


        }

        private void readPCInfo(StreamReader reader) {


            while (!reader.EndOfStream)
            {
                string strLine = reader.ReadLine();
                List<float> pc = new List<float>();

                String[] values = strLine.Split(Convert.ToChar(","));

                if (values[0].Equals(""))
                {
                    break;
                }
                else
                {
                    foreach (String str in values)
                    {
                        pc.Add(float.Parse(str));

                    }
                }

                pcs.Add(pc);
            }
            reader.Close();        
        }

        private void readColMeansInfo(StreamReader reader) { 
        
            string strLine = reader.ReadLine();

            String[] values = strLine.Split(Convert.ToChar(","));
            {
                foreach (String str in values)
                {
                    columnMeans.Add(float.Parse(str));

                }
            }
            reader.Close();        
        }

        private void readCovariances(StreamReader reader) {


            //while (!reader.EndOfStream)
            //{
            //    string strLine = reader.ReadLine();
            //    List<float> cov = new List<float>();

            //    String[] values = strLine.Split(Convert.ToChar(","));
            //    if (values[0].Equals(""))
            //    {
            //        break;
            //    }
            //    else
            //    {
            //        foreach (String str in values)
            //        {

            //            cov.Add(float.Parse(str));
            //        }
            //    }
            //    covariances.Add(cov);
            //}
            //reader.Close();          
        }

        private void createModel(String model, String covs) {
            GestureRegressionModel grm = new GestureRegressionModel();
            grm.readData(model, covs);
            gestureModel = grm;
            modelLength = grm.getLength();

            //change later!
            if (grm.getLength() >= endThresholdTime && endThresholdTime > 0 && startThresholdTime >= 0 && startThresholdTime < endThresholdTime)
            {
                modelLength = endThresholdTime - startThresholdTime + 1;
            }
            Console.WriteLine(gestureName + " model length is " + modelLength);
        }

        public bool checkForGMRMatch(List<float[]> data) {

            List<Tuple<float, float, float, float>> GMRValues = gestureModel.getGMRModelValues();
            List<List<float>> covariances = gestureModel.getCovariances();
            float totalDistanceSquared = 0;
            bool b = true;
            int startIndex = data.Count - modelLength;  //USES THE MOST RECENT VALUES
            //Console.WriteLine("data " + data.Count);
            //Console.WriteLine("gmr" + GMRValues.Count);
            //Console.WriteLine("model " + modelLength);

            for (int j = startIndex; j < data.Count; j++)
            {
                float[] inputVal = (float[])this.getInputVal(data[j]).Clone();   //GETS THE FIRST SET OF VALUES
                float[] timeInputVal = new float[inputVal.Count() + 1];          //CREATES NEW ARRAY WITH ADDED TIME VALUE

                timeInputVal[0] = j - startIndex + 1;                           //ENSURES FIRST TIME VALUE START FROM 1
                //     Console.WriteLine(timeInputVal[0]);
                inputVal.CopyTo(timeInputVal, 1);                               //COPIES TO NEW ARRAY        

                //foreach (float f in timeInputVal)
                //{
                //    if(gestureName.Equals("dribbleRight")){
                //    Console.WriteLine(f);
                //    }
                //}

                for (int i = 0; i < timeInputVal.Length; i++)
                {
                    timeInputVal[i] = timeInputVal[i] - columnMeans[i];       //SUBTRACTS COLUMN MEANS
                }

                float xVal = 0;
                float yVal = 0;

                for (int i = 0; i < timeInputVal.Length; i++)
                {
                    xVal = xVal + (timeInputVal[i] * pcs[0][i]);
                    yVal = yVal + (timeInputVal[i] * pcs[1][i]);             //DOES PCA ON VALUES
                }
                Tuple<float, float, float, float> comparisonData = GMRValues[j - startIndex];  //GETS GMR VALUE FOR COMPARISON
                List<float> cov = covariances[j - startIndex];                                 //GETS COVARIANCES FOR COMPARISON

                float dist = Gesture.getMahalanobisDistance(new float[] { xVal, yVal }, new float[] { comparisonData.Item2, comparisonData.Item3 }, cov);
                //FINDS DISTANCE BETWEEN BOTH
             //   Console.WriteLine(dist);

                if (dist > GMRthreshold)
                {
                    b = false;
             //       Console.WriteLine(j);
                    //if (gestureName.StartsWith("p"))
                    //{
                    //    Console.WriteLine("stopped at " + j);
                    //}
                    break;
                }

                //  double d = new Dtw(timeInputVal, timeInputVal);
            //    Console.WriteLine("sssss");
                totalDistanceSquared += dist;

            }

            if (b)
            {
                aveError = totalDistanceSquared / modelLength;
                Console.WriteLine(gestureName + "OK " + aveError);
                Console.WriteLine();
            }

            return false;
        }

        public bool checkForDTWDistance(List<float[]> data)
        {
            List<Tuple<float, float, float, float>> GMRValues = gestureModel.getGMRModelValues();
            bool b = true;
            int startIndex = data.Count - modelLength;  //USES THE MOST RECENT VALUES
            //Console.WriteLine("data " + data.Count);
            //Console.WriteLine("gmr" + GMRValues.Count);
            //Console.WriteLine("model " + modelLength);


            double[] DTWSeries = new double[modelLength];


            for (int j = startIndex; j < data.Count; j++)
            {
                float[] inputVal = (float[])this.getInputVal(data[j]).Clone();   //GETS THE FIRST SET OF VALUES
                float[] timeInputVal = new float[inputVal.Count() + 1];          //CREATES NEW ARRAY WITH ADDED TIME VALUE

                timeInputVal[0] = j - startIndex + 1 + startThresholdTime;      //ENSURES FIRST TIME VALUE START FROM START THRESHOLD
                //     Console.WriteLine(timeInputVal[0]);
                inputVal.CopyTo(timeInputVal, 1);                               //COPIES TO NEW ARRAY        

                //foreach (float f in timeInputVal)
                //{
                //    if(gestureName.Equals("dribbleRight")){
                //    Console.WriteLine(f);
                //    }
                //}

                for (int i = 0; i < timeInputVal.Length; i++)
                {
                    timeInputVal[i] = timeInputVal[i] - columnMeans[i];       //SUBTRACTS COLUMN MEANS
                }

                float xVal = 0;
                float yVal = 0;

                for (int i = 0; i < timeInputVal.Length; i++)
                {
                    xVal = xVal + (timeInputVal[i] * pcs[0][i]);
                    yVal = yVal + (timeInputVal[i] * pcs[1][i]);             //DOES PCA ON VALUES
                }
             //   Tuple<float, float, float, float> comparisonData = GMRValues[j - startIndex + startThresholdTime];  //GETS GMR VALUE FOR COMPARISON

                DTWSeries[j - startIndex] = yVal;

            }

            double d = this.doDTW(DTWSeries);
            this.setDTWValue(d);

            if (d < DTWThreshold)
            {
       //         Console.WriteLine(gestureName + " OK " + d);
                return true;
            }

            return false;

        }

        public void setDTWValue(double d) { 
            DTWValue = d;
        }

        public double getDTWValue() {
            return DTWValue;
        }

        public int getModelLength()
        {
            return modelLength;
        }

        public String getName() {
            return gestureName;
        }

        public float[] getInputVal(float[] dataLine) {

            if (dimensionsUsed.Equals("All"))
            {
                return dataLine;
            }
            else if(dimensionsUsed.Equals("Right Arm")){ 
            
                float[] f = new float[9];
                int startIndex = 9;

                for (int i = 0; i < f.Length; i++) {
                    f[i] = dataLine[startIndex];
                    startIndex++;
                }

                return f;
            
            
            }
            

           return dataLine;
        
        }

        public static float getMahalanobisDistance(float[] values, float[] means, List<float> covarianceMatrix) {


       //     Console.WriteLine(values[0] + " " + values[1] + " " + means[0] + " " + means[1] + " " + covarianceMatrix[0] + " " + covarianceMatrix[1] + " " + covarianceMatrix[2] + " " + covarianceMatrix[3]);
            float[] diffs = new float[] {values[0] - means[0], values[1] - means[1] };
      //      Console.Out.WriteLine(diffs[0] + " " + diffs[1]);
            float[] f = new float[2];

            f[0] = (diffs[0] * covarianceMatrix[0]) + (diffs[1] * covarianceMatrix[2]);
            f[1] = (diffs[0] * covarianceMatrix[1]) + (diffs[1] * covarianceMatrix[3]);

        //    Console.WriteLine(f[0] + " " + f[1]);

            float dist = (f[0] * diffs[0]) + (f[1] * diffs[1]);

       //     Console.Out.WriteLine(dist);


            return dist;
        
        }

        public float getAverageError() {
            return aveError;
        }

        public double doDTW(double[] x) {

            double[] y = new double[x.Length];

            List<Tuple<float, float, float, float>> GMRValues = gestureModel.getGMRModelValues();

            for (int i = 0; i < x.Length; i++) {
                y[i] = GMRValues[i + startThresholdTime].Item3;
            }

            double d = new Dtw(x,y, DistanceMeasure.Manhattan).GetCost();

            return d;
        }

        public double getDistFromModelPoint(int pointIndex, List<float[]> data)
        {

            List<Tuple<float, float, float, float>> GMRValues = gestureModel.getGMRModelValues();
            List<List<float>> covariances = gestureModel.getCovariances();
            int startIndex = data.Count - modelLength;  //USES THE MOST RECENT VALUES


            float[] inputVal = (float[])this.getInputVal(data[startIndex]).Clone();   //GETS THE FIRST SET OF VALUES
            float[] timeInputVal = new float[inputVal.Count() + 1];          //CREATES NEW ARRAY WITH ADDED TIME VALUE

            timeInputVal[0] = pointIndex + 1;                           //ENSURES FIRST TIME VALUE START FROM 1
            //     Console.WriteLine(timeInputVal[0]);
            inputVal.CopyTo(timeInputVal, 1);                               //COPIES TO NEW ARRAY        

            //foreach (float f in timeInputVal)
            //{
            //    if(gestureName.Equals("dribbleRight")){
            //    Console.WriteLine(f);
            //    }
            //}

            for (int i = 0; i < timeInputVal.Length; i++)
            {
                timeInputVal[i] = timeInputVal[i] - columnMeans[i];       //SUBTRACTS COLUMN MEANS
            }

            float xVal = 0;
            float yVal = 0;

            for (int i = 0; i < timeInputVal.Length; i++)
            {
                xVal = xVal + (timeInputVal[i] * pcs[0][i]);
                yVal = yVal + (timeInputVal[i] * pcs[1][i]);             //DOES PCA ON VALUES
            }
            Tuple<float, float, float, float> comparisonData = GMRValues[pointIndex];  //GETS GMR VALUE FOR COMPARISON
            List<float> cov = covariances[pointIndex];                                 //GETS COVARIANCES FOR COMPARISON

            return Gesture.getMahalanobisDistance(new float[] { xVal, yVal }, new float[] { comparisonData.Item2, comparisonData.Item3 }, cov);

//            Console.WriteLine(dist);

        }

        public int getStartThreshold() {
            return startThresholdTime;
        }

        public float getGMRThreshold() {
            return GMRthreshold;
        }

    }
}
